#include "PhysicCollisionDetection.h"

PhysicCollisionDetection::PhysicCollisionDetection(){};
PhysicCollisionDetection::~PhysicCollisionDetection(){};

void PhysicCollisionDetection::Update(PhysicRigidBody *a_RigidBody, LinkedList<PhysicRigidBody *> *a_RigidBodyList){
	int t_size = a_RigidBodyList->Get_Size();
	a_RigidBody->m_PhysicObject->m_PhysicObjectContactList.ClearPtr();
	a_RigidBody->m_PhysicRigidBodyContactList.ClearPtr();
	for(int j = 0; j < t_size; j++){
		PhysicRigidBody *t_RigidBody_2 = a_RigidBodyList->Get_Data(j);
		if(a_RigidBody == t_RigidBody_2){
			continue;
		}
		bool t_collide = false;

		t_collide = Detection(a_RigidBody, t_RigidBody_2);

		if(t_collide){
			bool t_insert = true;
			int t_sizeContactList = a_RigidBody->m_PhysicObject->m_PhysicObjectContactList.Get_Size();
			for(int k = 0; k < t_sizeContactList; k++){
				if(t_RigidBody_2->m_PhysicObject
					== (*a_RigidBody->m_PhysicObject->m_PhysicObjectContactList.Get_Data(k))){
						t_insert = false;
						break;
				}
			}
			if(t_insert){
				a_RigidBody->m_PhysicObject->m_PhysicObjectContactList.Insert_ToEnd(&(t_RigidBody_2->m_PhysicObject));
				a_RigidBody->m_PhysicRigidBodyContactList.Insert_ToEnd(t_RigidBody_2);
			}
		}
		t_RigidBody_2 = 0;
	}
	if(a_RigidBody->m_PhysicRigidBodyContactList.Get_Size() > 0){
		int sfg = 999231232;
	}
};

bool PhysicCollisionDetection::Detection(PhysicRigidBody *a_RigidBody_1, PhysicRigidBody *a_RigidBody_2){
	bool t_collide = false;

	switch(a_RigidBody_1->m_PhysicObject->m_physic_object_type){
	case PHYSIC_OBJECT_TYPE_CIRCLE:
		switch(a_RigidBody_2->m_PhysicObject->m_physic_object_type){
		case PHYSIC_OBJECT_TYPE_CIRCLE:
			t_collide = Detection_Circle_Circle(a_RigidBody_1, a_RigidBody_2);
			break;
		case PHYSIC_OBJECT_TYPE_SQUARE:
			t_collide = Detection_Circle_Square(a_RigidBody_1, a_RigidBody_2);
			break;
		}
		break;
	case PHYSIC_OBJECT_TYPE_SQUARE:
		switch(a_RigidBody_2->m_PhysicObject->m_physic_object_type){
		case PHYSIC_OBJECT_TYPE_CIRCLE:
			t_collide = Detection_Square_Circle(a_RigidBody_1, a_RigidBody_2);
			break;
		case PHYSIC_OBJECT_TYPE_SQUARE:
			t_collide = Detection_Square_Square(a_RigidBody_1, a_RigidBody_2);
			break;
		}
		break;
	}

	return t_collide;
};

bool PhysicCollisionDetection::Detection_Circle_Circle(PhysicRigidBody *a_RigidBody_1, PhysicRigidBody *a_RigidBody_2){
	bool t_collide = false;

	Vector2D t_Position_1 = a_RigidBody_1->m_PhysicObject->m_position;
	float t_radius_1 = a_RigidBody_1->m_PhysicObject->m_radius;
	Vector2D t_Position_2 = a_RigidBody_2->m_PhysicObject->m_position;
	float t_radius_2 = a_RigidBody_2->m_PhysicObject->m_radius;

	float t_distance = t_Position_1.distance(t_Position_2);
	if(t_distance < 0){
		t_distance *= -1;
	}
	if(t_distance <= (t_radius_1+t_radius_2)){
		t_collide = true;
	}

	return t_collide;
};

bool PhysicCollisionDetection::Detection_Square_Square(PhysicRigidBody *a_RigidBody_1, PhysicRigidBody *a_RigidBody_2){
	bool t_collide = false;

	Vector2D t_Position_1 = a_RigidBody_1->m_PhysicObject->m_position;
	float t_width_1 = a_RigidBody_1->m_PhysicObject->m_width;
	float t_height_1 = a_RigidBody_1->m_PhysicObject->m_height;
	Vector2D t_Position_2 = a_RigidBody_2->m_PhysicObject->m_position;
	float t_width_2 = a_RigidBody_2->m_PhysicObject->m_width;
	float t_height_2 = a_RigidBody_2->m_PhysicObject->m_height;

	//float t_distance = t_Position_1.distance(t_Position_2);
	//Vector2D t_normal = t_Position_1.plus(t_Position_2);
	//t_normal.normalize();

	if((abs(t_Position_1.x - t_Position_2.x) * 2 < (t_width_1 + t_width_2)) &&
		(abs(t_Position_1.y - t_Position_2.y) * 2 < (t_height_1 + t_height_2))){
			t_collide = true;
	}

	return t_collide;
};

bool PhysicCollisionDetection::Detection_Circle_Square(PhysicRigidBody *a_RigidBody_1, PhysicRigidBody *a_RigidBody_2){
	bool t_collide = false;

	Vector2D t_position_1 = a_RigidBody_1->m_PhysicObject->m_position;
	float t_radius_1 = a_RigidBody_1->m_PhysicObject->m_radius;
	Vector2D t_position_2 = a_RigidBody_2->m_PhysicObject->m_position;
	float t_width_2 = a_RigidBody_2->m_PhysicObject->m_width;
	float t_height_2 = a_RigidBody_2->m_PhysicObject->m_height;

	Vector2D circleDistance;
	circleDistance.x = abs(t_position_1.x - t_position_2.x);
	circleDistance.y = abs(t_position_1.y - t_position_2.y);

	do{
		if (circleDistance.x > (t_width_2/2 + t_radius_1)) { t_collide = false; break;}
		if (circleDistance.y > (t_height_2/2 + t_radius_1)) { t_collide = false; break;}

		if (circleDistance.x <= (t_width_2/2)) { t_collide = true; break;} 
		if (circleDistance.y <= (t_height_2/2)) { t_collide = true; break;}

		float cornerDistance_sq;
		cornerDistance_sq = (circleDistance.x - t_width_2/2)*(circleDistance.x - t_width_2/2) +
			(circleDistance.y - t_height_2/2)*(circleDistance.y - t_height_2/2);

		if(cornerDistance_sq <= (t_radius_1*t_radius_1)){
			t_collide = true;
		}else{
			t_collide = false;
		}

	}while(false);

	return t_collide;
};

bool PhysicCollisionDetection::Detection_Square_Circle(PhysicRigidBody *a_RigidBody_1, PhysicRigidBody *a_RigidBody_2){
	bool t_collide = false;

	Vector2D t_position_1 = a_RigidBody_1->m_PhysicObject->m_position;
	float t_width_1 = a_RigidBody_2->m_PhysicObject->m_width;
	float t_height_1 = a_RigidBody_2->m_PhysicObject->m_height;
	Vector2D t_position_2 = a_RigidBody_2->m_PhysicObject->m_position;
	float t_radius_2 = a_RigidBody_1->m_PhysicObject->m_radius;

	Vector2D circleDistance;
	circleDistance.x = abs(t_position_1.x - t_position_2.x);
	circleDistance.y = abs(t_position_1.y - t_position_2.y);

	do{
		if (circleDistance.x > (t_width_1/2 + t_radius_2)) { t_collide = false; break;}
		if (circleDistance.y > (t_height_1/2 + t_radius_2)) { t_collide = false; break;}

		if (circleDistance.x <= (t_width_1/2)) { t_collide = true; break;} 
		if (circleDistance.y <= (t_height_1/2)) { t_collide = true; break;}

		float cornerDistance_sq;
		cornerDistance_sq = (circleDistance.x - t_width_1/2)*(circleDistance.x - t_width_1/2) +
			(circleDistance.y - t_height_1/2)*(circleDistance.y - t_height_1/2);

		if(cornerDistance_sq <= (t_radius_2*t_radius_2)){
			t_collide = true;
		}else{
			t_collide = false;
		}

	}while(false);

	return t_collide;
};